﻿/**
 * Copyright (C), 2015-2045, Jimu
 *
 * File:         adas_out.c 
 * Description:  实现了一个脚本编译器和解释器, 通过脚本来控制CAN输出报文   
 * Others:
 * History:
 * - V0.1, 2018.06.19, LongLan
 *    Initial version
 *
 * - V0.2, 2018.08.10, LongLan
 *    1.增加了映射指令和映射表定义
 *    2.可以直接通过stdid()和extid()伪指令定义CAN报文名称, 不需要使用define指令定义CAN报文名称
 *    3.增加了限速输出
 *
 * - V0.3, 2018.10.17, LongLan
 *    1.改正了脚本编译器的bug, 在定义全局变量时, 全局变量分配索引没有加1, 导致所有全局变量使用一个内存空间
 */
#define CANO_VERSION  0x03

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include "adas_out.h"
#include "can_parse.h"


typedef unsigned char   u8;
typedef unsigned short  u16;
typedef unsigned int    u32;
typedef signed char     s8;
typedef signed short    s16;
typedef signed int      s32;


#undef  pr_err
#undef  pr_info
#define pr_err(fmt, ...)      printf("scrip: "fmt, ##__VA_ARGS__)
#define pr_info(fmt, ...)     printf("scrip: "fmt, ##__VA_ARGS__)



/* 定义操作数类型 */
#define OP_CONST     0  /* 常数 */
#define OP_TEMP      1  /* 临时变量或全局变量 */
#define OP_INPUT     2  /* 输入变量 */
#define OP_MAP       3  /* 映射表 */
#define OP_INVALID  -1  /* 非法的变量 */ 


/* 定义基本指令类型 */
#define BASE_INSTRUCTION_SIMPLE   0x0  /* 简单指令 */
#define BASE_INSTRUCTION_COMPLEX  0x3  /* 复杂指令 */
#define BASE_INSTRUCTION_ASSIGN   0x5  /* 算数运算指令 */
#define BASE_INSTRUCTION_LOGIC    0xA  /* 逻辑运算指令 */
#define BASE_INSTRUCTION_MISC     0xC  /* 混杂指令 */

/* 定义一部分指令 */
#define ELSE_   0x1
#define FI_     0x2
#define CLOSE_  0x3
#define EXIT_   0xF
#define OUT_    0x1
#define TIMER_  0x2
#define MAP_    0x3
#define IF_     0x1
#define ELIF_   0x2
#define AND_    0x3
#define OR_     0x4
#define FORCE_  0x1

 
/* 定义算数运算指令 */
#define OP_ASSIGN        0x1  /* op1  = op2 */
#define OP_ASSIGN_ADD    0x2  /* op1 += op2 */
#define OP_ASSIGN_MUL    0x3  /* op1 *= op2 */
#define OP_ASSIGN_DIV    0x4  /* op1 /= op2 */
#define OP_ASSIGN_SHR    0x5  /* op1 >>= op2 */
#define OP_ASSIGN_SHL    0x6  /* op1 <<= op2 */
#define OP_ASSIGN_XOR    0x7  /* op1 ^= op2 */
#define OP_ASSIGN_AND    0x8  /* op1 &= op2 */
#define OP_ASSIGN_OR     0x9  /* op1 |= op2 */

/* 定义关系运算符 */
#define OP_CMP_EQ       0x1  /* op1 == op2 */
#define OP_CMP_NQ       0x2  /* op1 != op2 */
#define OP_CMP_GT       0x3  /* op1 >  op2 */
#define OP_CMP_GE       0x4  /* op1 >= op2 */
#define OP_CMP_LT       0x5  /* op1 <  op2 */
#define OP_CMP_LE       0x6  /* op1 <= op2 */

/* 定义指令结构体 */
struct instruction_t {
	struct 
	{
		u16 type;
		u8  op1;
		u8  op2;
	} w1;
	s32 w2;
};

#define MAX_INSTRUCTIONS  256  /* 最大指令数目 */
#define MAX_CAN  16  /* ADAS最多输出CAN报文类型 */
#define TIMERS   16
struct script_exe_env {
	struct {
		int  valid;    /* CAN报文是否有效, 0:无效, 1:有效 */
		int  to_send;  /* CAN报文是否待发送, 0:不发送, 1:待发送 */
		u32  id;       /* CAN报文ID, bit31表示是否为扩展帧, 0:标准数据帧, 1:扩展数据帧 */
		u8   data[8];  /* CAN报文数据域 */
	} can[MAX_CAN];
	
	struct instruction_t instructions[MAX_INSTRUCTIONS];
	int  m[10 + 32];    /* 临时变量 + 全局变量 */
	int  timer_mod[TIMERS];  /* 定时器的模 */
	int  timer[TIMERS];      /* 定时器值 */
	int  instruction_cnt;  /* 实际编译后的指令个数 */
	int  script_disabled;  /* 停止脚本解释器 */
	int  nocheck;  /* 发送CAN报文时不检查CAN总线是否活跃 */
	int  object_enable;  /* 是否输出障碍物信息, 0=不输出, 1=输出 */
	int  car_signal_valid; /* 汽车信号是否有效, 如果有效则需要再执行脚本时获取汽车信号 */
	int  (*can_write)(unsigned int, unsigned char*, int);
};


struct alias_t {
	char name[64];  /* 宏名称 */
	int  op_type;   /* 操作数类型 */
	int  value;     /* 操作数值 */
};

struct alias_t *alias_table;
static int alias_entries;

struct key_word_t {
	const char *name;
	int  value;
};

#define GET_INDEX(member)  ((long)(&(((adas_out_t*)0)->member)) / sizeof(int))

/* 定义变量名称与变量索引值的映射关系 */
static const struct key_word_t  input_key[] = {
	{"fcw_level",    GET_INDEX(fcw_level) },
	{"hmw_level",    GET_INDEX(hmw_level) },
	{"ldw",          GET_INDEX(ldw) },
	{"headway",      GET_INDEX(headway) },
	{"ldw_on",       GET_INDEX(ldw_on) },
	{"car_on",       GET_INDEX(car_on) },
	{"ufcw",         GET_INDEX(ufcw) },
	{"car_follow",   GET_INDEX(car_follow) },
	{"drive_status", GET_INDEX(drive_status) },
	{"left_lane",    GET_INDEX(left_lane) },
	{"right_lane",   GET_INDEX(right_lane) },
	{"front_speed",  GET_INDEX(front_speed) },
	{"front_distance",  GET_INDEX(front_distance) },
	{"speed_limit",  GET_INDEX(speed_limit) },
	{"speed",        GET_INDEX(speed) },
	{"left_light",   GET_INDEX(left_light) },
	{"right_light",  GET_INDEX(right_light) },
	{"brake",        GET_INDEX(brake) },
};

/* 定义内置的关键字 */
#define STOP    0
#define START   1
#define RESTART 2
static const struct key_word_t  built_key[] = {
	{"STOP",     STOP},
	{"START",    START},
	{"RESTART",  RESTART},
};
	
	
static struct script_exe_env *adas_handle = NULL;

#define MAX_MAPS  16
#define MAX_MAP_KEYS  32
struct map_t {
	int key[MAX_MAP_KEYS][2];
	int key_num;
	int def_out;  /* 未找到匹配的输入时, 默认的输出值 */
} map_table[MAX_MAPS + 1];
static int map_table_num = 0;



static int    is_space(char c)
{
	if (c == ' ' || c == '\t' || c == ',')
		return 1;
	return 0;
}

static int    is_end(char c)
{
	if (c == '\0' || c == '\r' || c == '\n' || c == ';' || c == '#')
		return 1;
	return 0;
}

static int    is_variable(char c)
{
	if ( (c >= '0' && c <= '9') ||
		 (c >= 'a' && c <= 'z') ||
		 (c >= 'A' && c <= 'Z') ||
		 c == '_')
		 return 1;
	return 0;
}

static int    is_number(char c)
{
	if (c >= '0' && c <= '9')
		 return 1;
	return 0;
}

static int    get_middle_index(char *val)
{	
	if (val != NULL && val[0] == 'm' && val[2] == '\0' && val[1] >= '0' && val[1] <= '9') 
		return (val[1] - '0');
	
	return -1;
}


static int car_signal_valid = 0;
static int    get_input_index(char *val)
{
	int i;

	if (val == NULL)
		return -1;
	
	for (i = 0; i < sizeof(input_key) / sizeof(input_key[0]); i++)
	{
		if (strcmp(val, input_key[i].name) == 0) 
		{
			if (input_key[i].value >= GET_INDEX(speed) &&
			    input_key[i].value <= GET_INDEX(brake))
				car_signal_valid = 1;
			return input_key[i].value; 
		}
	}
	return -1;
}



/* 
 * 返回变量索引或常数值 
 * result: 常数
 *         临时变量
 *         输入变量
 *         操作数错误
*/
static int    __get_operation(char *val, int *result)
{
	int i, op, type;
	char *str;

	type = OP_INVALID;
	op = get_middle_index(val);
	if (op < 0) {
		op = get_input_index(val);
		if (op < 0) {
			op = strtol(val, &str, 0);
			if (*str == '\0')
				type = OP_CONST;
			else {
				for (i = 0; i < (sizeof(built_key) / sizeof(built_key[0])); i++)
				{
					if (strcmp(val, built_key[i].name) == 0) {
						type = OP_CONST;
						op   = built_key[i].value;
						break;
					}
				}
			}
		} else {
			type = OP_INPUT;
		}
	} else {
		type = OP_TEMP;
	}

	if (result != NULL)
		*result = type;

    return op;
}



static int    get_operation(char *val, int *result)
{
	int i, op, type;

	op = __get_operation(val, &type);
	if (type == OP_INVALID) {
		for (i = 0; i < alias_entries; i++) 
		{
			if (strcmp(val, alias_table[i].name) == 0) {
				op = alias_table[i].value;
				type = alias_table[i].op_type;
				break;
			}
		}
	}

	if (result != NULL)
		*result = type;

    return op;
}


static int    get_instruction_string(char *in, char** out, int limit)
{
	int strs, start, is_val;
	int c_num;
	char *s;

	c_num = 0;
	strs = 0;
	start = 1;
	s = *out++;
	while (is_end(*in) == 0 && strs <= limit && c_num <= 1024) {
		if (start != 0) {
			if (is_space(*in) == 0) {
				start = 0;
				if ( (*in == '-' || *in == '+') && is_number(*(in+1)) ) /* 在这里把有符号常数也当做变量 */
					is_val = is_variable('0');
				else
				    is_val = is_variable(*in); 
				*s++ = *in;
			}
			in++;
			c_num++;
		} else {
			if (is_space(*in) || is_val != is_variable(*in) ) {
				*s = '\0';
				s = *out++;
				strs++;
				start = 1;
			} else {
				*s++ = *in++;
				c_num++;
			}
		}
	}

	if (start == 0) {
		strs++;
		*s = '\0';
	}

	return strs;
}



/*
 * < 0: 语法错误
 * = 0: 结束
 * > 0: 找到关键词
*/
static int    get_map_key(const char *input, char **key, char **end)
{
	int start, strs;
	char *s;
	
	while (*input != '\0' && is_space(*input))
		input++;
	if (*input != '[') {
		if (end)
		    *end = (char*)input;
		return (*input == '\0')? 0 : -1;
	}
	input++;
	
	start = 1;
	strs = 0;
	s = *key++;
	while (*input != '\0' && *input != ']')
	{
		if (start != 0) {
			if (is_space(*input) == 0) {
				if (strs >= 2)
					break;
				start = 0;
				*s++ = *input;
			}
			input++;
		} else {
			if (is_space(*input)) {
				*s = '\0';
				s = *key++;
				strs++;
				start = 1;
			} else {
				*s++ = *input++;
			}
		}		
	}
	if (end)
		*end = (char*)input;
	if (*input != ']')
		return -1;
    if (end)
		(*end)++;

	if (start == 0) {
		strs++;
		*s = '\0';
	}
	return (strs == 0)? -1 : strs;
}


/*
 * < 0: 映射表语法错误
 * = 0: 不是映射表
 * > 0: 映射表查找成功
*/
static int    get_map_table(char *line, struct map_t *map, char *map_name)
{
	char *str, *end;
	char key[2][64], *key_ptr[2];
	int c_num = 0, i, key_num, cnt;
	int key_val[2];
	
	str = line;
	while (is_end(*str) == 0 && c_num <= 1024)
	{
		str++;
		c_num++;
	}
	*str = '\0';
	
	while (*line != '\0' && isspace(*line))
		line++;
	if (*line == '\0')
		return 0;
	if (strncmp(line, "mapdef", 6) || isspace(line[6]) == 0)
		return 0;
	line += 7;
	
	while (*line != '\0' && isspace(*line))
		line++;
	str = line;
	while (*str != '\0' && isspace(*str) == 0 && *str != '[')
		if (is_variable(*str++) == 0)
			return -1;
	c_num = str - line;
	if (*str == '\0' || c_num == 0 || c_num > 32)
		return -1;
	memcpy(map_name, line, c_num);
	map_name[c_num] = '\0';	
	line += c_num;
	
	str = line;
	cnt = 0;
	key_num = 0;
	key_ptr[0] = key[0];
	key_ptr[1] = key[1];
	while ( (cnt = get_map_key(str, key_ptr, &end)) > 0)
	{
		for (i = 0; i < cnt; i++)
		{
			key_val[i] = strtol(key[i], &str, 0);
			if (*str != '\0')
				return -1;
		}
		str = end;
		if (cnt == 1)
			map->def_out = key_val[0];
		else {
			if (key_num >= MAX_MAP_KEYS)
				return -1;
			map->key[key_num][0] = key_val[0];
			map->key[key_num][1] = key_val[1];
			++key_num;
		}
	}
	if (cnt < 0 || key_num == 0)
		return -1;
	map->key_num = key_num;
	return 1;
}


static void    simple_instruction(struct instruction_t *ins, int type) {
	ins->w1.type = 0xA000 | BASE_INSTRUCTION_SIMPLE | (0x5 << 8)| (type << 4);
	ins->w1.op1 = 0xFF;
	ins->w1.op2 = 0xFF;
	ins->w2 = 0;
}


#define MAX_SYNTAX_UNITS  64
#define MAX_STRINGS  64
#define MAX_ALIAS   (16 + 16 + 32)  /* 可以定义16个宏,16个映射表和32个全局变量 */
int    open_adas_script(const char *script, int (*can_write)(unsigned int, unsigned char*, int))
{
	struct script_exe_env *adas;
	FILE *infile;
	int i, line_cnt, syntax_cnt;
	char *syntax[MAX_SYNTAX_UNITS], *str_buffer;
	char line_buf[1024];
	char map_name[64];
	int if_index, notes;
	int ret;
	int sub_instruction, ext_instruction, w2, instruction_cnt; 
	int op1, op2, op_type, tmp;
	int timer_index, timer_start;
	int global_index;
	int msg_id_index;

	enum {i_if, i_elif, i_else, i_fi, i_out, i_stdid, i_extid, i_nop, i_assign, i_exit, 
	      i_timer, i_close, i_define, i_global, i_force, i_nocheck, i_object, i_map} cmd;

	if (script == NULL || *script == '\0' || can_write == NULL) {
		pr_err("Empty file name or empty can write function!\n");
		return -1;
	}
	infile = fopen(script, "rt");
	if (!infile)
	{
		pr_err("%s is not exist!\r\n", script);
		return -1;
	}
	
	map_table_num = 0;
	memset(map_table, 0, sizeof(map_table));
	
	alias_entries = 0;
    alias_table = (struct alias_t*)malloc(sizeof(struct alias_t) * MAX_ALIAS);
	if (!alias_table) {
		fclose(infile);
		pr_err("Allocate memory failure\n");
		return -1;
	}
	
	adas = (struct script_exe_env*)malloc(sizeof(struct script_exe_env));
    if (!adas) {
		fclose(infile);
		free(alias_table);
		pr_err("Allocate memory failure\n");
		return -1;
	}
	memset(adas, 0, sizeof(*adas) );
	adas->can_write = can_write;

	str_buffer = (char*)malloc(MAX_SYNTAX_UNITS * MAX_STRINGS);
	if (!str_buffer) {
		fclose(infile);
		free(adas);
		free(alias_table);
		pr_err("Allocate memory failure\n");
		return -1;
	}

	for (i = 0; i < MAX_SYNTAX_UNITS; i++)
		syntax[i] = str_buffer + i * MAX_STRINGS; 

	notes = 0;
	if_index = 0;
	timer_index = 0;
	timer_start = 0;
	global_index = 0;
	msg_id_index = 0;
	instruction_cnt = 0;
	ret = 0;
	line_cnt = 0;
	while (fgets(line_buf, 1024, infile) != NULL && instruction_cnt < MAX_INSTRUCTIONS) {
		if ( (line_cnt == 0) && memcmp(line_buf, "#!", 2) != 0) {
			pr_err("The first line must begin with '#!'\n");
			ret = -1;
			break;
		}
		line_cnt++;
		
		syntax_cnt = get_instruction_string(line_buf, syntax, MAX_SYNTAX_UNITS);
        if (syntax_cnt == 0) continue;
        
		/* 跳过注释区域 */
		if (notes == 0) {
			if (syntax[0][0] == '/' && syntax[0][1] == '*') {
				if (strcmp(syntax[syntax_cnt-1], "*/") != 0)
				    notes = 1;
				continue;
			}
		} else {
			int str_len = strlen(syntax[syntax_cnt-1]);
			if (str_len > 1 && syntax[syntax_cnt-1][str_len-1] == '/' && syntax[syntax_cnt-1][str_len-2] == '*') 
				notes = 0;
			continue;
		}
		
		/* 获取映射表 */
		int map_valid = 0;
		if (syntax_cnt >= 2 && strcmp(syntax[0], "mapdef") == 0)
		    map_valid = get_map_table(line_buf, map_table + map_table_num, map_name);
		if (map_valid < 0) {
			pr_err("Line%d: Mapdef Syntax error\n", line_cnt);
			ret = -1;
			break;
		} else if (map_valid > 0) {
			if (map_table_num >= MAX_MAPS) {
				pr_err("Line%d: Mapdef is too much\n", line_cnt);
			    ret = -1;
				break;
			} else if (alias_entries >= MAX_ALIAS) {
				pr_err("Line%d: Lack of resources for mapdef\n", line_cnt);
				ret = -1;
				break;
			} else {
				get_operation(map_name, &op_type);
				if (op_type != OP_INVALID) {
				    pr_err("Line%d: Map definition name conflict\n", line_cnt);
				    ret = -1;
					break;
			    }
			}
			alias_table[alias_entries].op_type = OP_MAP;
			alias_table[alias_entries].value = map_table_num++;
			strncpy(alias_table[alias_entries].name, map_name, 64);
			alias_entries++;
			continue;
		}

		if (strcmp(syntax[0], "if") == 0)           cmd = i_if;
		else if (strcmp(syntax[0], "elif") == 0)    cmd = i_elif;
		else if (strcmp(syntax[0], "else") == 0)    cmd = i_else;
		else if (strcmp(syntax[0], "fi") == 0)      cmd = i_fi;
		else if (strcmp(syntax[0], "out") == 0)     cmd = i_out;
		else if (strcmp(syntax[0], "stdid") == 0)   cmd = i_stdid;
		else if (strcmp(syntax[0], "extid") == 0)   cmd = i_extid;
		else if (strcmp(syntax[0], "nop") == 0)     cmd = i_nop;
		else if (strcmp(syntax[0], "exit") == 0)    cmd = i_exit;
		else if (strcmp(syntax[0], "define") == 0)  cmd = i_define;
		else if (strcmp(syntax[0], "global") == 0)  cmd = i_global;
		else if (strcmp(syntax[0], "timer") == 0)   cmd = i_timer;
		else if (strcmp(syntax[0], "close") == 0)   cmd = i_close;
		else if (strcmp(syntax[0], "force") == 0)   cmd = i_force;
		else if (strcmp(syntax[0], "nocheck") == 0) cmd = i_nocheck;
		else if (strcmp(syntax[0], "object") == 0)  cmd = i_object;
		else if (strcmp(syntax[0], "map") == 0)     cmd = i_map;
		else                                        cmd = i_assign;

		if (cmd == i_assign) { /* 算数运算 */
			if (syntax_cnt != 3) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
		
			sub_instruction  = 0;
			op1 = 0xFF;
			op2 = op1;
			w2 = 0;

            /* 检查运算符 */
			if (strcmp(syntax[1], "=") == 0)        sub_instruction = OP_ASSIGN;
			else if (strcmp(syntax[1], "+=") == 0)  sub_instruction = OP_ASSIGN_ADD;
			else if (strcmp(syntax[1], "*=") == 0)  sub_instruction = OP_ASSIGN_MUL;
			else if (strcmp(syntax[1], "/=") == 0)  sub_instruction = OP_ASSIGN_DIV;
			else if (strcmp(syntax[1], ">>=") == 0) sub_instruction = OP_ASSIGN_SHR;
			else if (strcmp(syntax[1], "<<=") == 0) sub_instruction = OP_ASSIGN_SHL;
			else if (strcmp(syntax[1], "^=") == 0)  sub_instruction = OP_ASSIGN_XOR;
			else if (strcmp(syntax[1], "&=") == 0)  sub_instruction = OP_ASSIGN_AND;
			else if (strcmp(syntax[1], "|=") == 0)  sub_instruction = OP_ASSIGN_OR;
			else {
				pr_err("Line%d: Operator error\n", line_cnt);
				ret = -1;
				break;
			}

			/* 检查操作数1, 必须为临时变量 */
			op1 = get_operation(syntax[0], &op_type);
			if (op_type != OP_TEMP) {
				pr_err("Line%d: Op1 must be middle variable\n", line_cnt);
				ret = -1;
				break;
			}
			op1 |= 0x80;

			/* 检查操作数2, 可以是输入变量、临时变量、常数 */
			op2 = get_operation(syntax[2], &op_type);
			if (op_type == OP_CONST) {
				w2  = op2;
				op2 = 0xFF;
				if (sub_instruction == OP_ASSIGN_DIV && w2 == 0) {
					pr_err("Line%d: Divisor can not be 0\n", line_cnt);
				    ret = -1;
				    break;
				}
			} else if (op_type == OP_TEMP) {
				op2 |= 0x80;
			} else if (op_type != OP_INPUT) {
				pr_err("Line%d: Op2 error\n", line_cnt);
				ret = -1;
				break;
			}

			adas->instructions[instruction_cnt].w1.type = 0xA000u | BASE_INSTRUCTION_ASSIGN | (sub_instruction << 4) | (5 << 8);
			adas->instructions[instruction_cnt].w1.op1 = op1;
			adas->instructions[instruction_cnt].w1.op2 = op2;
			adas->instructions[instruction_cnt].w2 = w2;
			instruction_cnt++;
		} else if (cmd == i_nocheck) {  /* 输出CAN报文时不检查CAN总线是否活跃 */
		    if (syntax_cnt != 1) {
				pr_err("Line%d: Nocheck Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			adas->nocheck = 1;
		} else if (cmd == i_object) {  /* 输出障碍物信息和车道线信息 */
		    if (syntax_cnt != 1) {
				pr_err("Line%d: object Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			adas->object_enable = 1;
		} else if (cmd == i_force) {  /* 强制CAN输出 */
		    if (syntax_cnt != 2) {
				pr_err("Line%d: Force Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			op1 = get_operation(syntax[1], &op_type);
			if (op_type != OP_CONST || op1 >= MAX_CAN) {
				pr_err("Line%d: Force parament error\n", line_cnt);
				ret = -1;
				break;
			}
			adas->instructions[instruction_cnt].w1.type = 0xA000u | BASE_INSTRUCTION_MISC | (FORCE_ << 4) | (5 << 8);
			adas->instructions[instruction_cnt].w1.op1 = 0xFF;
			adas->instructions[instruction_cnt].w1.op2 = 0xFF;
			adas->instructions[instruction_cnt].w2 = op1;
			instruction_cnt++;
		} else if (cmd == i_define) {  /* 宏定义伪指令 */
		    if (alias_entries >= MAX_ALIAS) {
				pr_err("Line%d: Macros define quantity exceeding limits\n", line_cnt);
				ret = -1;
				break;
			}
			if (syntax_cnt != 3) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			get_operation(syntax[1], &op_type);
			if (op_type != OP_INVALID) {
				pr_err("Line%d: Macro definition name error\n", line_cnt);
				ret = -1;
				break;
			}
			op1 = __get_operation(syntax[2], &op_type);
			if (op_type == OP_INVALID) {
				pr_err("Line%d: Macro defined value errors\n", line_cnt);
				ret = -1;
				break;
			}
			alias_table[alias_entries].op_type = op_type;
			alias_table[alias_entries].value = op1;
			strncpy(alias_table[alias_entries].name, syntax[1], 64);
			alias_entries++;
		} else if (cmd == i_global) {  /* 定义全局变量 */
		    if (alias_entries >= MAX_ALIAS || global_index >= 32) {
				pr_err("Line%d: Global define quantity exceeding limits\n", line_cnt);
				ret = -1;
				break;
			}
			if (syntax_cnt != 3) {
				pr_err("Line%d: Global Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			get_operation(syntax[1], &op_type);
			if (op_type != OP_INVALID) {
				pr_err("Line%d: Global name redefinition\n", line_cnt);
				ret = -1;
				break;
			}
			op1 = __get_operation(syntax[2], &op_type);
			if (op_type != OP_CONST) {
				pr_err("Line%d: Global initial value is not constant\n", line_cnt);
				ret = -1;
				break;
			}
			adas->m[global_index + 10] = op1;
			alias_table[alias_entries].op_type = OP_TEMP;
			alias_table[alias_entries].value = global_index + 10;
			strncpy(alias_table[alias_entries].name, syntax[1], 64);
			alias_entries++;
			global_index++;
		} else if (cmd == i_stdid || cmd == i_extid) {  /* 指定CAN报文类型的伪指令 */
			if (syntax_cnt != 5 || strcmp(syntax[1], "(") != 0 || strcmp(syntax[4], ")") != 0) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			op1 = get_operation(syntax[2], &op_type);
			if (op_type == OP_CONST) {
				if (op1 >= msg_id_index)
					msg_id_index = op1 + 1;
			} else if (op_type == OP_INVALID) {  /* 重新定义一个CAN报文变量 */
			    if (alias_entries >= MAX_ALIAS) {
				    pr_err("Line%d: quantity of can msg name exceeding limits\n", line_cnt);
				    ret = -1;
				    break;
			    }
			    op1 = msg_id_index++;
			    alias_table[alias_entries].op_type = OP_CONST;
			    alias_table[alias_entries].value = op1;
			    strncpy(alias_table[alias_entries].name, syntax[2], 64);
				alias_entries++;
			} else {
				pr_err("Line%d: can msg name error\n", line_cnt);
				ret = -1;
				break;
			}

			op2 = get_operation(syntax[3], &op_type);
			if (op_type != OP_CONST) {
				pr_err("Line%d: Op2 is not constant\n", line_cnt);
				ret = -1;
				break;
			}

			if (op1 >= MAX_CAN) {
				pr_err("Line%d: Wrong Can index\n", line_cnt);
				ret = -1;
				break;
			}
            if (cmd == i_extid)
				op2 |= 0x80000000;
			adas->can[op1].id = op2;
			adas->can[op1].valid = 1;
		} else if (cmd == i_if || cmd == i_elif) {  /* 1=if  2=elif  3=else */
			if ( (cmd == i_if && if_index != 0) ||
				 (cmd == i_elif && if_index != 1 && if_index != 2) || 
				 (syntax_cnt % 6) != 0 ) {
				pr_err("Line%d: 'if-else' nesting or order error\n", line_cnt);
				ret = -1;
				break;
			}

			if (cmd == i_if) if_index = 1;
			else             if_index = 2;
			for (i = 0; i < syntax_cnt; i += 6) {
				if (strcmp(syntax[i+3], "==") == 0)      ext_instruction = OP_CMP_EQ;
				else if (strcmp(syntax[i+3], "!=") == 0) ext_instruction = OP_CMP_NQ;
				else if (strcmp(syntax[i+3], ">") == 0)  ext_instruction = OP_CMP_GT;
				else if (strcmp(syntax[i+3], ">=") == 0) ext_instruction = OP_CMP_GE;
				else if (strcmp(syntax[i+3], "<") == 0)  ext_instruction = OP_CMP_LT;
				else if (strcmp(syntax[i+3], "<=") == 0) ext_instruction = OP_CMP_LE;
				else {
					pr_err("Line%d: Unsupported symbols: %s\n", line_cnt, syntax[i+3]);
					ret = -1;
					break;
				}
				if (strcmp(syntax[i+1], "(") != 0 || strcmp(syntax[i+5], ")") != 0) {
					pr_err("Line%d: Syntax error\n", line_cnt);
			    	ret = -1;
				    break;
				}

				if (strcmp(syntax[i], "and") == 0)     sub_instruction = AND_;
				else if (strcmp(syntax[i], "or") == 0) sub_instruction = OR_;
				else if (i != 0) {
					pr_err("Line%d: Syntax error\n", line_cnt);
					ret = -1;
					break;
				} else {
					if (cmd == i_if) sub_instruction = IF_;
					else             sub_instruction = ELIF_;
				}
                w2 = 0;
				/* 获取操作数 */
				op1 = get_operation(syntax[i+2], &op_type);
				if (op_type == OP_CONST) {
					w2  = op1;
					op1 = 0xFF;
				} else if (op_type == OP_TEMP) {
					op1 |= 0x80;
				} else if (op_type != OP_INPUT) {
				    pr_err("Line%d: Parameter error\n", line_cnt);
				    ret = -1;
				    break;
				}

				op2 = get_operation(syntax[i+4], &op_type);
				if (op_type == OP_CONST) {
					w2  = op2;
					op2 = 0xFF;
				} else if (op_type == OP_TEMP) {
					op2 |= 0x80;
				} else if (op_type != OP_INPUT) {
				    pr_err("Line%d: Parameter error\n", line_cnt);
				    ret = -1;
				    break;
				}

				if (op1 == op2) {
					pr_err("Line%d: Op1 is the same as Op2, or both are constant\n", line_cnt);
				    ret = -1;
				    break;
				}

				adas->instructions[instruction_cnt].w1.type = 0xA000 | BASE_INSTRUCTION_LOGIC | (sub_instruction << 4) | (ext_instruction << 8);
			    adas->instructions[instruction_cnt].w1.op1 = op1;
			    adas->instructions[instruction_cnt].w1.op2 = op2;
			    adas->instructions[instruction_cnt].w2 = w2;
			    instruction_cnt++;

				if (instruction_cnt >= MAX_INSTRUCTIONS) {
					pr_err("Instruction area overflow\n");
				    ret = -1;
				    break;
				}
			}
			if (ret != 0)
				break;
		} else if (cmd == i_timer) {
			if (timer_start != 0 || if_index != 0) {
				pr_err("Line%d: Timer is not end or if-else-fi is not end\n", line_cnt);
				ret = -1;
				break;
			}
			if (timer_index >= TIMERS) {
				pr_err("Line%d: The number of timers exceeds the limit\n", line_cnt);
				ret = -1;
				break;
			}
			if (syntax_cnt != 5 && strcmp(syntax[1], "(") != 0 || strcmp(syntax[4], ")") != 0) {
				pr_err("Line%d: Timer syntax error\n", line_cnt);
				ret = -1;
				break;
			}

			w2 = 0;
			op1 = get_operation(syntax[2], &op_type);
			if (op_type == OP_CONST) {
				if (op1 != STOP && op1 != START && op1 != RESTART)  {
					pr_err("Line%d: Parameter error\n", line_cnt);
				    ret = -1;
				    break;
				}
				w2 = op1;
				op1 = 0xFF;
			} else if (op_type == OP_TEMP) 
				op1 |= 0x80;
			else {
				pr_err("Line%d: Parameter error\n", line_cnt);
				ret = -1;
				break;
			}
			
			op2 = get_operation(syntax[3], &op_type);
			if (op_type != OP_CONST) {
				pr_err("Line%d: Timer cycle must be constant\n", line_cnt);
				ret = -1;
				break;
			}
			if (op2 < 10 || op2 > 10000)  {
				pr_err("Line%d: Timer cycle error %dms\n", line_cnt, op2);
				ret = -1;
				break;
			}
			adas->timer_mod[timer_index] = op2;
			adas->instructions[instruction_cnt].w1.type = 0xA000 | BASE_INSTRUCTION_COMPLEX | (TIMER_ << 4) | (5 << 8);
			adas->instructions[instruction_cnt].w1.op1 = op1;
			adas->instructions[instruction_cnt].w1.op2 = timer_index;
			adas->instructions[instruction_cnt].w2 = w2;
			timer_start = 1;
			instruction_cnt++;
			timer_index++;
		} else if (cmd == i_out) {
			if (syntax_cnt != 7 && strcmp(syntax[1], "(") != 0 || strcmp(syntax[6], ")") != 0) {
				pr_err("Line%d: Out syntax error\n", line_cnt);
				ret = -1;
				break;
			}

			tmp = 0;
			op1 = get_operation(syntax[2], &op_type);
			if (op_type == OP_CONST) {
				if (op1 < 0 || op1 > 255) {
					pr_err("Line%d: Parameter error\n", line_cnt);
				    ret = -1;
				    break;
				}
				tmp = op1;
				op1 = 0xFF;
			}
			else if (op_type == OP_TEMP)  op1 |= 0x80;
			else if (op_type != OP_INPUT) {
				pr_err("Line%d: Parameter error\n", line_cnt);
				ret = -1;
				break;
			}
			op2 = get_operation(syntax[3], &op_type);
			if (op_type != OP_CONST) {
				pr_err("Line%d: Can index must be constant\n", line_cnt);
				ret = -1;
				break;
			}
			if (op2 >= MAX_CAN || adas->can[op2].valid == 0) {
				pr_err("Line%d: Can index undefined\n", line_cnt);
				ret = -1;
				break;
			}
			w2 = get_operation(syntax[4], &op_type);
			if (op_type != OP_CONST || w2 >= 63) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			i = get_operation(syntax[5], &op_type);
			if (op_type != OP_CONST || i < 1 || i > 8) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			if ( (w2 + i) > 64 || ( (w2 + i - 1) / 8 != w2 / 8) ) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}
			w2 |= (i << 8) | (tmp << 16);
			adas->instructions[instruction_cnt].w1.type = 0xA000 | BASE_INSTRUCTION_COMPLEX | (OUT_ << 4) | (5 << 8);
			adas->instructions[instruction_cnt].w1.op1 = op1;
			adas->instructions[instruction_cnt].w1.op2 = op2;
			adas->instructions[instruction_cnt].w2 = w2;
			instruction_cnt++;
		} else if (cmd == i_map) {
			if (syntax_cnt != 6 && strcmp(syntax[1], "(") != 0 || strcmp(syntax[5], ")") != 0) {
				pr_err("Line%d: Map syntax error\n", line_cnt);
				ret = -1;
				break;
			}

			tmp = 0;
			op1 = get_operation(syntax[3], &op_type);
			if (op_type == OP_TEMP)  op1 |= 0x80;
			else if (op_type != OP_INPUT) {
				pr_err("Line%d: op1 error\n", line_cnt);
				ret = -1;
				break;
			}
			op2 = get_operation(syntax[4], &op_type);
			if (op_type != OP_TEMP) {
				pr_err("Line%d: op2 error\n", line_cnt);
				ret = -1;
				break;
			}
			op2 |= 0x80;
			w2 = get_operation(syntax[2], &op_type);
			if (op_type != OP_MAP || w2 >= map_table_num) {
				pr_err("Line%d: Map is not exist\n", line_cnt);
				ret = -1;
				break;
			}
			
			adas->instructions[instruction_cnt].w1.type = 0xA000 | BASE_INSTRUCTION_COMPLEX | (MAP_ << 4) | (5 << 8);
			adas->instructions[instruction_cnt].w1.op1 = op1;
			adas->instructions[instruction_cnt].w1.op2 = op2;
			adas->instructions[instruction_cnt].w2 = w2;
			instruction_cnt++;
		} else {  /* 简单指令 */
			if (syntax_cnt != 1) {
				pr_err("Line%d: Syntax error\n", line_cnt);
				ret = -1;
				break;
			}

			if (cmd == i_else) {  /* 1=if  2=elif  3=else */
				if (if_index != 1 && if_index != 2) {
					pr_err("Line%d: 'if-else' order error\n", line_cnt);
				    ret = -1;
				    break;
				}
                if_index = 3;
				simple_instruction(adas->instructions + instruction_cnt, ELSE_);
				instruction_cnt++;
			} else if (cmd == i_fi) {
				if (if_index == 0) {
					pr_err("Line%d: Syntax error\n", line_cnt);
				    ret = -1;
				    break;
				}
				if_index = 0;
				simple_instruction(adas->instructions + instruction_cnt, FI_);
				instruction_cnt++;
			} else if (cmd == i_exit) {  /* 退出程序 */
				simple_instruction(adas->instructions + instruction_cnt, EXIT_);
				instruction_cnt++;
			} else if (cmd == i_close) {  /* 退出定时执行区域 */
			    if (timer_start == 0 || if_index != 0) {
					pr_err("Line%d: Timer is not start or if-else-fi is not end\n", line_cnt);
				    ret = -1;
				    break;
				}
				timer_start = 0;
				simple_instruction(adas->instructions + instruction_cnt, CLOSE_);
				instruction_cnt++;
			} else if (cmd != i_nop) {
			    pr_err("Line%d: Unsupported instruction\n", line_cnt);
			    ret = -1;
			    break;
			} 
		}
	}

	if (ret == 0) {
		if (if_index != 0) {
			pr_err("Lack of 'fi'\n");
			ret = -1;
		} else if (timer_start != 0) {
			pr_err("Lack of 'close for timer'\n");
			ret = -1;
		} else if (instruction_cnt == 0) {
			pr_info("Empty script\n");
		}

		if (instruction_cnt >= MAX_INSTRUCTIONS) {
		    pr_err("Instruction area overflow\n");
		    ret = -1;
		} else {
			simple_instruction(adas->instructions + instruction_cnt, EXIT_);
		    instruction_cnt++;
			adas->instruction_cnt = instruction_cnt;
		}
	}		
	
	fclose(infile);
	free(str_buffer);
	free(alias_table);
	alias_entries = 0;
	alias_table = NULL;
    
	adas->car_signal_valid = car_signal_valid;
	car_signal_valid = 0;
	if (ret == 0)
	    adas_handle = adas;
	else
		free(adas);

	return ret;
}


void    close_adas_script(void)
{
	if (adas_handle == NULL)
		return;
	free(adas_handle);
	adas_handle = NULL;
}

#if 0
void    pr_script(void)
{
	struct script_exe_env *adas;
	struct instruction_t *ins; 
	int i;

	if (adas_handle == NULL)
	    return;
	adas = adas_handle;
	printf("CAN ID List:\n");
	for (i = 0; i < MAX_CAN; i++)
	{
		if (adas->can[i].valid != 0)
			printf("%d: 0x%08x\n", i, adas->can[i].id);
	}
	printf("Instructions: %d\n", adas->instruction_cnt);
    ins = adas->instructions;

	printf("base sub ext  op1  op2   w2\n");
	for (i = 0; i < adas->instruction_cnt; i++, ins++)
	{
		printf("0x%x  0x%x 0x%x  0x%02x 0x%02x  0x%08x  %d\n",
			    ins->w1.type & 0xf, (ins->w1.type >> 4) & 0xf, (ins->w1.type >> 8) & 0xf,
				ins->w1.op1, ins->w1.op2, ins->w2,ins->w2);
	}
}
#endif




int    script_execute(adas_out_t *output, struct can_msg_task *task, int inc_ms)
{
	struct script_exe_env *adas;
	struct instruction_t *ins; 
	signal_t car_signal;
	int i, ret;
	int *m, *input;
	struct map_t *map;
	int base_instruction, sub_instruction; 
	int op1, op2;
	int master_logic, local_logic, skip_logic, timer_logic;

	if (adas_handle == NULL)
	    return -1;
	adas = adas_handle;
	if (adas->script_disabled)
		return 0;

    if (adas_handle->nocheck == 0 && can_is_alive() == 0)
    	return 0;
    
    /* 发送障碍物信息的CAN报文和车道线CAN报文 */
    if (adas->object_enable && task != NULL && task->msg_count > 0)	{
		for (i = 0; i < task->msg_count; i++)
			adas->can_write(task->msg[i].id, task->msg[i].data, 8);
	}
	
	ins = adas->instructions;
	for (i = 0; i < MAX_CAN; i++)
		if (adas->can[i].valid)
			memset(adas->can[i].data, 0, 8);

	ret = 0;
	m = adas->m;
	input = (int*)output;
	master_logic = 1;
	timer_logic = 1;
	skip_logic = 0;
	
	/* 获取汽车信号 */
	if (adas->car_signal_valid && can_get_input(&car_signal) == 0) {
		output->speed = car_signal.speed < 0? 0 : car_signal.speed;
		output->brake = car_signal.brake;
		output->left_light = car_signal.left_light;
		output->right_light = car_signal.right_light;
	}
	
	for (i = 0; i < adas->instruction_cnt; i++, ins++)
	{
		if ( (ins->w1.type >> 12) != 0xA) {
			ret = -1;
			pr_err("INS%d: Instruction Magic error\n", i);
			break;
		}

		base_instruction = ins->w1.type & 0xF;
		sub_instruction  = (ins->w1.type >> 4) & 0xF;
        op1 = ins->w1.op1;
		op2 = ins->w1.op2;
		if (base_instruction == BASE_INSTRUCTION_SIMPLE) { /* 简单指令 */
			if (sub_instruction == CLOSE_) {  /* 退出定时执行区域 */
				timer_logic = 1;
				continue;
			}
			
			if (timer_logic == 0) continue;
			if (sub_instruction == ELSE_) {
				if (skip_logic == 0 && master_logic == 0)
					master_logic = 1;
				else
					master_logic = 0;
			} else if (sub_instruction == FI_) {
				master_logic = 1;
				skip_logic   = 0;
			} else {  /* exit */
				if (master_logic != 0)  /* 退出脚本 */
					break;
			}
		} else if (base_instruction == BASE_INSTRUCTION_MISC) {  /* 混杂指令 */
		    if (timer_logic == 0) continue;
			if (master_logic == 0) continue;
			
			if (sub_instruction == FORCE_) {
				adas->can[ins->w2].to_send = 1;
			} 
		} else if (base_instruction == BASE_INSTRUCTION_COMPLEX) { /* 复杂指令 */
			if (timer_logic == 0) continue;
			if (master_logic == 0) continue;

			if (sub_instruction == OUT_) {
				int byte_index, byte_shift, byte_mask;

				if (op1 == 0xFF)     op1 = (ins->w2 >> 16) & 0xFF;
			    else if (op1 & 0x80) op1 = m[op1 & 0x7F];
			    else                 op1 = input[op1];
				byte_index = (ins->w2 & 0x3F) >> 3;
				byte_shift = ins->w2 & 7;
				byte_mask  = (ins->w2 >> 8) & 0xF;
                byte_mask  = ( (1 << byte_mask) - 1) << byte_shift;
				op1 = (op1 << byte_shift) & byte_mask;
				adas->can[op2].data[byte_index] = adas->can[op2].data[byte_index] & (~byte_mask) | op1;
				adas->can[op2].to_send = 1;
			} else if (sub_instruction == TIMER_) {
				if (op1 == 0xFF)  op1 = ins->w2;
			    else              op1 = m[op1 & 0x7F];
				
				timer_logic = 0;
				if (op1 == RESTART) {
					adas->timer[op2] = adas->timer_mod[op2];
					timer_logic = 1;
				} else if (op1 == START) {
					adas->timer[op2] -= inc_ms;
					if (adas->timer[op2] <= 0) {
						timer_logic = 1;
					    adas->timer[op2] = adas->timer_mod[op2];
					}
				} else {
					adas->timer[op2] = 0;
				}
			} else if (sub_instruction == MAP_) {
				if (op1 & 0x80) op1 = m[op1 & 0x7F];
			    else            op1 = input[op1];
				map = &map_table[ins->w2];
				op2 &= 0x7F;
				
				int index;
				for (index = 0; index < map->key_num; index++)
					if (map->key[index][0] == op1)
						break;
				if (index < map->key_num)
					m[op2] = map->key[index][1];
				else
					m[op2] = map->def_out;
			}
		} else if (base_instruction == BASE_INSTRUCTION_ASSIGN) {  /* 算数运算指令 */
			if (timer_logic == 0) continue;
			if (master_logic == 0) continue;

			if (op2 == 0xFF)     op2 = ins->w2;
			else if (op2 & 0x80) op2 = m[op2 & 0x7F];
			else                 op2 = input[op2];

			op1 &= 0x7F;
			switch (sub_instruction) {
			case OP_ASSIGN:
				m[op1] = op2;
				break;
			case OP_ASSIGN_ADD:
				m[op1] += op2;
				break;
			case OP_ASSIGN_MUL:
				m[op1] *= op2;
			    break;
			case OP_ASSIGN_DIV:
				if (op2 == 0) {
					ret = -1;
		        	pr_err("INS%d: Divisor can not be 0\n", i);
				} else {
					m[op1] /= op2;
				}
				break;
			case OP_ASSIGN_SHR:
				m[op1] >>= op2;
				break;
			case OP_ASSIGN_SHL:
				m[op1] <<= op2;
				break;
			case OP_ASSIGN_XOR:
				m[op1] ^= op2;
				break;
			case OP_ASSIGN_AND:
				m[op1] &= op2;
				break;
			case OP_ASSIGN_OR:
				m[op1] |= op2;
				break;
			}

		} else if (base_instruction == BASE_INSTRUCTION_LOGIC) {   /* 逻辑运算指令 */
			if (timer_logic == 0) continue;
			if (skip_logic != 0) continue;

			if (op1 == 0xFF)     op1 = ins->w2;
			else if (op1 & 0x80) op1 = m[op1 & 0x7F];
			else                 op1 = input[op1];

			if (op2 == 0xFF)     op2 = ins->w2;
			else if (op2 & 0x80) op2 = m[op2 & 0x7F];
			else                 op2 = input[op2];

			local_logic  = 0;
            switch ((ins->w1.type >> 8) & 0xF) {
			case OP_CMP_EQ:
				if (op1 == op2) local_logic = 1;
				break;
			case OP_CMP_NQ:
				if (op1 != op2) local_logic = 1;
				break;
			case OP_CMP_GT:
				if (op1 > op2)  local_logic = 1;
				break;
			case OP_CMP_GE:
				if (op1 >= op2) local_logic = 1;
				break;
			case OP_CMP_LT:
				if (op1 < op2)  local_logic = 1;
				break;
			case OP_CMP_LE:
				if (op1 <= op2) local_logic = 1;
				break;
			}

			if (sub_instruction == IF_) {
				master_logic = local_logic;
			} else if (sub_instruction == ELIF_) {
				if (master_logic != 0) {
					skip_logic = 1;
					master_logic = 0;
				} else
					master_logic = local_logic;
			} else if (sub_instruction == AND_) {
				if (local_logic == 0)
					master_logic = 0;
			} else {  /* or */
				if (local_logic != 0)
					master_logic = 1;
			}

		} else {
			ret = -1;
			pr_err("INS%d: Base Instruction error\n", i);
			break;
		}
	}

	if (ret == 0) {
		for (i = 0; i < MAX_CAN; i++)
			if (adas->can[i].to_send) {
			    adas->can_write(adas->can[i].id, adas->can[i].data, 8);
				adas->can[i].to_send = 0;
			}
	}

	return ret;
}


unsigned char  get_canoutput_version(void)
{
	return CANO_VERSION;
}
